/**
 * 
 */
package ch.unizh.ifi.ailab.ai;

import org.apache.log4j.Logger;

import ch.unizh.ifi.ailab.config.ProgramSettings;
import ch.unizh.ifi.ailab.dataTypes.Command;
import ch.unizh.ifi.ailab.dataTypes.SensorData;
import ch.unizh.ifi.ailab.dataTypes.Vector2D;


/**
 * @author hans jaeckle jaeckle@gmail.com
 * 
 */
public class SMCSquareRepulsive extends SensorMotorCoupling {

	// properties
	private static ProgramSettings myProp = ProgramSettings
			.getProgramSettings();

	// steps from center to one extrema
	private int steeringSteps = myProp.getIntProperty("dirMax")
			- myProp.getIntProperty("dirCenter");

	// angles
	double angleS1 = Math.toRadians(myProp.getIntProperty("sensor1Angle"));

	double angleS2 = Math.toRadians(myProp.getIntProperty("sensor2Angle"));

	double angleS3 = Math.toRadians(myProp.getIntProperty("sensor3Angle"));

	double angleS4 = Math.toRadians(myProp.getIntProperty("sensor4Angle"));

	// logging
	private static Logger myLog = Logger.getLogger(AICore.class);

	public SMCSquareRepulsive() {
		myLog
				.debug("smc: initialisiere square repulsive vector smc.");
	}

	public void calculateMotionCommand(SensorData data, Command command)
			throws UnavoidableObjectException {
		Vector2D resultingVector = new Vector2D(0, 0);
		double newSpeed = myProp.getIntProperty("speedStop");
		double newDir = myProp.getIntProperty("dirCenter");

		if (isFrontWallCase(data) || isCloseObjectsituation(data)) {
			resultingVector = calculateRepulsiveForceFrontWallCase(data);

			// speed calc:
			newSpeed += getSpeedConstWallCase()
					/ Math.sqrt(resultingVector.getNorm()) + 1;

			if (newSpeed - command.getSpeed() >= 2) {
				newSpeed = command.getSpeed() + 2;
			}

			// (-1)!!! suitcase direction has changed after building in the new
			// speed controller...
			// normalerweise muss dies gleichung noch mit *(-1) erweitert
			// werden,
			// aber da der roboter "flasch" steuert ist das gut ohne!

			newDir += Math.toDegrees(resultingVector.getAngle())
					* steeringSteps / getMaxResultingAngleWallCase();
		} else if (isSideWallCase(data) || isCloseObjectsituation(data)) {
			resultingVector = calculateRepulsiveForceFrontWallCase(data);

			// speed calc:
			newSpeed += getSpeedConstSideWallCase()
					/ Math.sqrt(resultingVector.getNorm()) + 1;

			if (newSpeed - command.getSpeed() >= 2) {
				newSpeed = command.getSpeed() + 2;
			}

			// (-1)!!! suitcase direction has changed after building in the new
			// speed controller...
			// normalerweise muss dies gleichung noch mit *(-1) erweitert
			// werden,
			// aber da der roboter "flasch" steuert ist das gut ohne!
			newDir += Math.toDegrees(resultingVector.getAngle())
					* steeringSteps / getMaxResultingAngleSideWallCase();
		} else {
			resultingVector = calculateRepulsiveForce(data);

			// speed calc:
			newSpeed += getSpeedConst() / Math.sqrt(resultingVector.getNorm());

			if (newSpeed - command.getSpeed() >= 2) {
				newSpeed = command.getSpeed() + 2;
			}

			// (-1)!!! suitcase direction has changed after building in the new
			// speed controller...
			// normalerweise muss dies gleichung noch mit *(-1) erweitert
			// werden,
			// aber da der roboter "flasch" steuert ist das gut ohne!
			newDir += Math.toDegrees(resultingVector.getAngle())
					* steeringSteps / getMaxResultingAngle();
		}

		// notstop oder kommando setzen:
		if (data.getIo1() < getSecurityDist()
				|| data.getIo2() < getSecurityDist() * getSecurityDistFactor()
				|| data.getIo3() < getSecurityDist() * getSecurityDistFactor()
				|| data.getIo4() < getSecurityDist()) {
			throw new UnavoidableObjectException("smc: panik; werte: 1:"
					+ data.getIo1() + " 2:" + data.getIo2() + " 3:"
					+ data.getIo3() + " 4:" + data.getIo4()
					+ " security distance: " + getSecurityDist());
		} else {
			// set command
			command.setSpeed((int) Math.round(newSpeed));
			command.setDirection((int) Math.round(newDir));
		}
	}

	/**
	 * detects a situation that could be a approaching wall in front or on only
	 * one side.
	 * 
	 * @param data
	 *            current sensor data.
	 * @return true if situation could be a wall, false if not.
	 */
	private boolean isFrontWallCase(SensorData data) {
		// wall case = sensor1 and 4 > 2 and 3
		// and |sensor2 - sensor3| < (sensor2+sensor3) / 2 * wallCasePercentage
		int s1 = data.getIo1();
		int s2 = data.getIo2();
		int s3 = data.getIo3();
		int s4 = data.getIo4();

		// situations approaching walls
		double diff2 = Math.abs(s2 - s3);
		double av2 = (s2 + s3) / 2;

		if (s1 > s2 && s4 > s3 && av2 < getMaxWallCaseDist()
				&& diff2 <= av2 * getWallCasePercentage()) {
			// wall in front:
			myLog.info("smc: detected possible wall in front.");
			return true;
		}
		return false;
	}

	private boolean isCloseObjectsituation(SensorData data) {
		// wall case = sensor1 and 4 > 2 and 3
		// and |sensor2 - sensor3| < (sensor2+sensor3) / 2 * wallCasePercentage
		int s1 = data.getIo1();
		int s4 = data.getIo4();

		// very close object
		if (s1 < getCloseObjectSituation() || s4 < getCloseObjectSituation()) {
			// wall on the right
			myLog.info("smc: detected close object");
			return true;
		}
		return false;
	}

	private boolean isSideWallCase(SensorData data) {
		// wall case = sensor1 and 4 > 2 and 3
		// and |sensor2 - sensor3| < (sensor2+sensor3) / 2 * wallCasePercentage
		int s1 = data.getIo1();
		int s2 = data.getIo2();
		int s3 = data.getIo3();
		int s4 = data.getIo4();

		// situations approaching walls
		double diff1 = Math.abs(s1 - s2);
		double av1 = (s1 + s2) / 2;
		double diff3 = Math.abs(s3 - s4);
		double av3 = (s3 + s4) / 2;
		if (av1 < getMaxSideWallCaseDist() && diff1 <= av1 * getWallCasePercentage()
				&& s2 < s3) {
			// wall on the left
			myLog.info("smc: detected possible wall on the left");
			return true;
		} else if (av3 < getMaxSideWallCaseDist()
				&& diff3 <= av3 * getWallCasePercentage() && s2 > s3) {
			// wall on the right
			myLog.info("smc: detected possible wall on the right");
			return true;
		}
		return false;
	}

	private Vector2D calculateRepulsiveForceSideWallCase(SensorData data) {
		Vector2D force = new Vector2D(0, 0);

		// verst�rkung der reaktion durch verdoppelung der l�ngeren distanz
		int s1 = data.getIo1();
		int s2 = data.getIo4();
		int s3 = data.getIo3();
		int s4 = data.getIo4();

		Vector2D force3 = new Vector2D(0, 0);
		//
		// if (s1 < s2) {
		// s2 = 2 * s2;
		// if (s2 > 255) {
		// s2 = 255;
		// }
		// } else if (s1 > s2) {
		// s1 = 2 * s1;
		// if (s1 > 255) {
		// s1 = 255;
		// }
		// }

		Vector2D forceS1 = new Vector2D(calculateSensorVector(s1, angleS1));
		Vector2D forceS4 = new Vector2D(calculateSensorVector(s2, angleS4));

		if (s3 < s4) {
			force3 = new Vector2D(s3, angleS3);
		} else if (s3 > s4) {
			force3 = new Vector2D(s4, angleS4);
		}

		force.plus(forceS1);
		force.plus(force3);
		force.plus(forceS4);

		return force;
	}

	private Vector2D calculateRepulsiveForceFrontWallCase(SensorData data) {
		Vector2D force = new Vector2D(0, 0);

		// verst�rkung der reaktion durch verdoppelung der l�ngeren distanz
		int s1 = data.getIo1();
		int s4 = data.getIo4();

		if (s1 < s4) {
			s4 = 2 * s4;
			if (s4 > 255) {
				s4 = 255;
			}
		} else if (s1 > s4) {
			s1 = 2 * s1;
			if (s1 > 255) {
				s1 = 255;
			}
		}

		Vector2D forceS1 = new Vector2D(calculateSensorVector(s1, angleS1));
		Vector2D forceS4 = new Vector2D(calculateSensorVector(s4, angleS4));

		force.plus(forceS1);
		force.plus(forceS4);

		return force;
	}

	private Vector2D calculateRepulsiveForce(SensorData data) {
		Vector2D force = new Vector2D(0, 0);

		Vector2D forceS1 = new Vector2D(calculateSensorVector(data.getIo1(),
				angleS1));
		Vector2D forceS2 = new Vector2D(calculateSensorVector(data.getIo2(),
				angleS2));
		Vector2D forceS3 = new Vector2D(calculateSensorVector(data.getIo3(),
				angleS3));
		Vector2D forceS4 = new Vector2D(calculateSensorVector(data.getIo4(),
				angleS4));

		force.plus(forceS1);
		force.plus(forceS2);
		force.plus(forceS3);
		force.plus(forceS4);

		return force;
	}

	private Vector2D calculateSensorVector(int distance, double angle) {
		double xVal = (255 * Math.sin(angle))
				/ Math.pow((distance * Math.sin(angle)), 2);
		double yVal = (255 * Math.cos(angle))
				/ Math.pow((distance * Math.cos(angle)), 2);
		return new Vector2D(xVal, yVal);
	}

	private double getSecurityDistFactor() {
		return myProp.getDoubleProperty("securityDistFactor");
	}

	private double getSpeedConst() {
		return myProp.getDoubleProperty("speedConst");
	}

	private int getSecurityDist() {
		return myProp.getIntProperty("securityDist");
	}

	private double getMaxResultingAngle() {
		return myProp.getDoubleProperty("maxResultingAngle");
	}

	private double getWallCasePercentage() {
		return myProp.getDoubleProperty("wallCasePercentage");
	}

	private int getCloseObjectSituation() {
		return myProp.getIntProperty("closeObjectSituation");
	}

	private double getMaxResultingAngleWallCase() {
		return myProp.getDoubleProperty("maxResultingAngleWallCase");
	}

	private double getMaxResultingAngleSideWallCase() {
		return myProp.getDoubleProperty("maxResultingAngleSideWallCase");
	}

	private int getMaxWallCaseDist() {
		return myProp.getIntProperty("maxWallCaseDist");
	}

	private int getMaxSideWallCaseDist() {
		return myProp.getIntProperty("maxSideWallCaseDist");
	}

	private double getSpeedConstWallCase() {
		return myProp.getDoubleProperty("speedConstWallCase");
	}

	private double getSpeedConstSideWallCase() {
		return myProp.getDoubleProperty("speedConstSideWallCase");
	}
}
